#include "pid.h"

void positional_pid_init(positional_pid_t *pid, const float kp, const float ki, const float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->target = 0.0f;
    pid->actual = 0.0f;
    pid->error = 0.0f;
    pid->error_last = 0.0f;
    pid->integral = 0.0f;
    pid->integral_limit = 1000.0f; // 默认积分限幅
    pid->output = 0.0f;
    pid->output_limit = 1000.0f; // 默认输出限幅
}

float positional_pid_calculate(positional_pid_t *pid, const float target, const float actual)
{
    pid->target = target;
    pid->actual = actual;
    pid->error = pid->target - pid->actual;

    // 积分项
    pid->integral += pid->error;
    // 积分限幅
    if (pid->integral > pid->integral_limit)
    {
        pid->integral = pid->integral_limit;
    }
    else if (pid->integral < -pid->integral_limit)
    {
        pid->integral = -pid->integral_limit;
    }

    /* PID计算 */
    pid->output = pid->kp * pid->error +
                  pid->ki * pid->integral +
                  pid->kd * (pid->error - pid->error_last);

    /* 输出限幅 */
    if (pid->output > pid->output_limit)
    {
        pid->output = pid->output_limit;
    }
    else if (pid->output < -pid->output_limit)
    {
        pid->output = -pid->output_limit;
    }

    pid->error_last = pid->error;

    return pid->output;
}

void incremental_pid_init(incremental_pid_t *pid, const float kp, const float ki, const float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->target = 0.0f;
    pid->actual = 0.0f;
    pid->error = 0.0f;
    pid->error_last = 0.0f;
    pid->error_prev = 0.0f;
    pid->output = 0.0f;
    pid->output_limit = 1000.0f; // 默认输出限幅
}


float incremental_pid_calculate(incremental_pid_t *pid, const float target, const float actual)
{
    pid->target = target;
    pid->actual = actual;
    pid->error = pid->target - pid->actual;

    // 增量计算
    float increment = pid->kp * (pid->error - pid->error_last) +
                      pid->ki * pid->error +
                      pid->kd * (pid->error - 2 * pid->error_last + pid->error_prev);

    // 更新输出
    pid->output += increment;

    // 输出限幅
    if (pid->output > pid->output_limit)
    {
        pid->output = pid->output_limit;
    }
    else if (pid->output < -pid->output_limit)
    {
        pid->output = -pid->output_limit;
    }

    // 更新误差记录
    pid->error_prev = pid->error_last;
    pid->error_last = pid->error;

    return pid->output;
}
